﻿using System;
using System.Collections.Generic;
using System.Text;
using GeometryCSharp;
using AGHFiraPlayer.Drivers;
using AGHFiraPlayer.PreviousDriver;

namespace AGHFiraPlayer.Strategies.StrategiesImpl
{
    class DeffensiveSweeperStrategy : AbstractStrategy
    {
        private const double CONFLICT_DISTANCE = AGHDriver.ROBOT_DIAMETER * 2;

        public DeffensiveSweeperStrategy(ref AGHDriver driver)
        {
            involvedRobots = new List<RobotController>(1);
            this.driver = driver;
        }

        override public int applicabilityLevel(ref List<IStrategy> choosenStrategies)
        {
            foreach (IStrategy strat in choosenStrategies)
                if (strat.GetType().Name.Contains("Sweeper"))
                {
                    foreach (RobotController robot in driver.Robots)
                        if (!robot.isUsedInStrategy)
                        {
                            involvedRobots.Clear();
                            involvedRobots.Add(robot);
                            break;
                        }
                    currentApplicability = 1;
                    return currentApplicability;
                }

            Point2D ballPos = new Point2D(driver.firaDriver.ballX, driver.firaDriver.ballY);

            List<RobotController> sortedOpponents = Predicator.getOpponentListSortedByDistanceFromGivenPoint(ballPos);
            double opponentBallDistance = sortedOpponents[0].Position.GetDistance(driver.firaDriver.ballX, driver.firaDriver.ballY);
            List<RobotController> sortedRobots = Predicator.getRobotListSortedByDistanceFromGivenPoint(getUnoccupiedRobots(), ballPos);
            double robotBallDistance = sortedRobots[0].Position.GetDistance(driver.firaDriver.ballX, driver.firaDriver.ballY);

            RobotController closestFacingRobot = sortedRobots[0];
            int i = 1;
            while (ballPos.x < closestFacingRobot.Position.x && i < sortedRobots.Count)
                closestFacingRobot = sortedRobots[i++];

            //second condition only for tests (when sortedRobots.count == 1)
            if (i == sortedRobots.Count && ballPos.x < sortedRobots[i - 1].Position.x)
            {
                involvedRobots.Clear();
                involvedRobots.Add(sortedRobots[0]);
                currentApplicability = MAX_APPLICABILITY;
                return currentApplicability;
            }

            i--;
            double tangens = Math.Abs(Math.Tan(Math.Abs(sortedRobots[i].Position.y - ballPos.y) / Math.Abs(sortedRobots[i].Position.x - ballPos.x)));
            if (tangens > 3)
                tangens = 3;
            double ballDistance = sortedRobots[i].Position.GetDistance(ballPos);
            double bestCoefficient = tangens / ballDistance;

            currentApplicability = calculateApplicability(opponentBallDistance, robotBallDistance, tangens);

            for (int j = i + 1; j < sortedRobots.Count; j++)
            {
                tangens = Math.Tan(Math.Abs(sortedRobots[j].Position.y - ballPos.y) / Math.Abs(sortedRobots[j].Position.x - ballPos.x));
                if (tangens > 3)
                    tangens = 3;
                ballDistance = sortedRobots[j].Position.GetDistance(ballPos);

                if (tangens / ballDistance > bestCoefficient)
                {
                    bestCoefficient = tangens / ballDistance;
                    closestFacingRobot = sortedRobots[j];
                    currentApplicability = calculateApplicability(opponentBallDistance, robotBallDistance, tangens);
                }
            }

            involvedRobots.Clear();
            involvedRobots.Add(closestFacingRobot);
            return currentApplicability;
        }

        private int calculateApplicability(double opponentBallDistance, double robotBallDistance, double tangens)
        {
            return 7;
        }

        override public void execute()
        {
            double positionError = AGHDriver.ROBOT_DIAMETER;
            double maximumDistanceToKick = AGHDriver.ROBOT_DIAMETER;

            Point2D robotPosition = new Point2D(involvedRobots[0].robotState.x, involvedRobots[0].robotState.y);
            Point2D intersectionPoint = Predicator.findBestIntersectionPoint(robotPosition);
            Point2D ballPos = new Point2D(driver.firaDriver.ballX, driver.firaDriver.ballY);

            Boolean intersectionPointToFarAway = (double.IsNaN(intersectionPoint.x) || double.IsNaN(intersectionPoint.y)) ? true : false;

            if (ballPos.x < involvedRobots[0].Position.x)
            {
                GoalKeeperDriver.DriveVelocity(involvedRobots[0].robotState, involvedRobots[0].startupPosition.x, involvedRobots[0].startupPosition.y, PotencialDriver.maxVelocity);
            }
            else if (intersectionPointToFarAway)
            {
                GoalKeeperDriver.DriveVelocity(involvedRobots[0].robotState, ballPos.x, ballPos.y, PotencialDriver.maxVelocity);
            }
            else
            {
                if (robotPosition.GetDistance(intersectionPoint) > positionError)
                    GoalKeeperDriver.DriveVelocity(involvedRobots[0].robotState, intersectionPoint.x, intersectionPoint.y, PotencialDriver.maxVelocity);
                else
                    fixRobotBallAngleAndRotateKick(ballPos, robotPosition, maximumDistanceToKick);
            }
        }

        private void goBack()
        {
        }

        private void fixRobotBallAngleAndRotateKick(Point2D ballPos, Point2D robotPosition, double maximumDistanceToKick)
        {
            double pitchHalfHeight = AGHDriver.pitchArea.Height / 2; ;
            Boolean isInLeftWingSector = ballPos.y < pitchHalfHeight ? true : false;

            SimpleMovingFunctions.rotateToBall(involvedRobots[0], driver.firaDriver);
            if (robotPosition.GetDistance(ballPos) < maximumDistanceToKick)
                SimpleMovingFunctions.rotateKick(involvedRobots[0], isInLeftWingSector);
        }
    }
}
